#!/usr/bin/env python

# EBand Local Planner Dynamic Reconfigure

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# common parameters
gen.add("xy_goal_tolerance", double_t, 0, "Distance tolerance for reaching the goal pose", 0.1, 0.0)
gen.add("yaw_goal_tolerance", double_t, 0, "Orientation tolerance for reaching the desired goal pose", 0.05, 0.0)
gen.add("rot_stopped_vel", double_t, 0, "Angular velocity lower bound that determines if the robot should stop to avoid limit-cycles or locks", 0.01, 0.0)
gen.add("trans_stopped_vel", double_t, 0, "Linear velocity lower bound that determines if the robot should stop to avoid limit-cycles or locks", 0.01, 0.0)

# visualization parameters
gen.add("marker_lifetime", double_t, 0, "Lifetime of eband visualization markers", 0.5, 0.0)

# eband local planner parameters
gen.add("eband_min_relative_overlap", double_t, 0, "Min distance that denotes connectivity between consecutive bubbles", 0.7, 0.0)
gen.add("eband_tiny_bubble_distance", double_t, 0, "Bubble geometric bound regarding tiny bubble distance", 0.01, 0.0)
gen.add("eband_tiny_bubble_expansion", double_t, 0, "Bubble geometric bound regarding tiny bubble expansion", 0.01, 0.0)
gen.add("eband_internal_force_gain", double_t, 0, "Force gain of forces between consecutive bubbles that tend to stretch the elastic band", 1.0, 0.0)
gen.add("eband_external_force_gain", double_t, 0, "Force gain of forces that tend to move the bubbles away from obstacles", 2.0, 0.0)
gen.add("num_iterations_eband_optimization", int_t, 0, "Number of iterations for eband optimization", 3, 1)
gen.add("eband_equilibrium_approx_max_recursion_depth", int_t, 0, "Number of iterations for reaching the equilibrium between internal and external forces", 4, 1)
gen.add("eband_equilibrium_relative_overshoot", double_t, 0, "Maximum relative equlibrium overshoot", 0.75, 0.0)
gen.add("eband_significant_force_lower_bound", double_t, 0, "Minimum magnitude of force that is considered significant and used in the calculations", 0.15, 0.0)
gen.add("costmap_weight", double_t, 0, "Costmap weight factor used in the calculation of distance to obstacles", 10.0, 0.0)

# trajectory controller parameters
gen.add("max_vel_lin", double_t, 0, "Maximum linear velocity", 0.75, 0.0)
gen.add("max_vel_th", double_t, 0, "Maximum angular velocity", 1.0, 0.0)
gen.add("min_vel_lin", double_t, 0, "Minimum linear velocity", 0.1, 0.0)
gen.add("min_vel_th", double_t, 0, "Minimum angular velocity", 0.0, 0.0)
gen.add("min_in_place_vel_th", double_t, 0, "Minimum in-place angular velocity", 0.0, 0.0)
gen.add("in_place_trans_vel", double_t, 0, "Minimum in place linear velocity", 0.0, 0.0)
gen.add("k_prop", double_t, 0, "Proportional gain of the PID controller", 4.0, 0.0)
gen.add("k_damp", double_t, 0, "Damping gain of the PID controller", 3.5, 0.0)
gen.add("Ctrl_Rate", double_t, 0, "Control rate", 10.0, 0.0)
gen.add("max_acceleration", double_t, 0, "Maximum allowable acceleration", 0.5, 0.0)
gen.add("virtual_mass", double_t, 0, "Virtual mass", 0.75, 0.0)
gen.add("max_translational_acceleration", double_t, 0, "Maximum linear acceleration", 0.5, 0.0)
gen.add("max_rotational_acceleration", double_t, 0, "Maximum angular acceleration", 1.5, 0.0)
gen.add("rotation_correction_threshold", double_t, 0, "Rotation correction threshold", 0.5, 0.0)
gen.add("differential_drive", bool_t, 0, "Denotes whether to use the differential drive hack", True)
gen.add("bubble_velocity_multiplier", double_t, 0, "Multiplier of bubble radius", 2.0, 0.0)
gen.add("rotation_threshold_multiplier", double_t, 0, "Multiplier of rotation threshold", 1.0, 0.0)
gen.add("disallow_hysteresis", bool_t, 0, "Determines whether to try getting closer to the goal, in case of going past the tolerance", False)

exit(gen.generate("eband_local_planner", "eband_local_planner", "EBandPlanner"))
